#include "../inc/mzrtos.h"
#include "../inc/mzhw.h"

mz_thread_t mz_current_thread = MZ_NULL;
struct mz_list_node mz_thread_priority_list[MZ_THREAD_PRIORITY_MAX];

/* This function is used to find the highest priority thread in the thread priority table */
mz_thread_t _mz_scheduler_search_highest_priority(void){
    mz_thread_t thread = MZ_NULL;
    mz_uint32_t i;

    for(i = 0; i < MZ_THREAD_PRIORITY_MAX; i++){
        if(mz_thread_priority_list[i].next != &mz_thread_priority_list[i]){
            thread = mz_list_entry(mz_thread_priority_list[i].next,
                                  struct mz_thread,
                                  list);
            return thread;
        }
    }

    return MZ_NULL;
}

/* The content of this function is only for debugging purposes, and it will change in the next version */
void mz_schedule(void){
    mz_thread_t thread_from;
    mz_thread_t thread_to;

    thread_from = mz_current_thread;

    thread_to = _mz_scheduler_search_highest_priority();

    if(thread_to != MZ_NULL){
        mz_current_thread = thread_to;
        mz_hw_context_switch((mz_uint32_t)&thread_from->sp, (mz_uint32_t)&thread_to->sp);
    }
}

/* After entering this function, the scheduler will start working and will not return */
void mz_scheduler_start(void){
    mz_uint32_t level;
    mz_thread_t thread;

    level = mz_hw_interrupt_disable();
    thread = _mz_scheduler_search_highest_priority();

    if(thread != MZ_NULL){
        mz_current_thread = thread;

        mz_hw_first_switch_to((mz_uint32_t)&(mz_current_thread->sp));
    }

    mz_hw_interrupt_enable(level);
}

void mz_scheduler_insert_thread(mz_thread_t thread){
    mz_uint32_t level;

    level = mz_hw_interrupt_disable();

    mz_list_insert_before(&mz_thread_priority_list[thread->current_priority], &thread->list);

    mz_hw_interrupt_enable(level);
}

void mz_scheduler_remove_thread(mz_thread_t thread){
    mz_uint32_t level;

    level = mz_hw_interrupt_disable();

    mz_list_remove(&thread->list);

    mz_hw_interrupt_enable(level);
}

void mz_scheduler_system_init(void){
    mz_uint32_t i;
    for(i = 0; i < MZ_THREAD_PRIORITY_MAX; i++){
        mz_list_init(&mz_thread_priority_list[i]);
    }
}